
#include "member_BSixdofRestraint.h"
#include "member_BRestraint.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

member_BSixdofRestraint::member_BSixdofRestraint(BSixdofRestraint* restraint)
{
	boss = restraint;
	for (int i = 0; i < 6; i++)
	{
		springEnabled[i] = false;
		equilibriumPoint[i] = 0;
		springStiffness[i] = 0;
		springDamping[i] = 1;
	}
}
member_BSixdofRestraint::~member_BSixdofRestraint()
{

}

void member_BSixdofRestraint::calculateTransforms()
{
	BBody* bodyA = restraint_member(boss)->bodyA;
	BBody* bodyB = restraint_member(boss)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	BVector pointA = matrixA * restraint_member(boss)->pivotA;
	BVector pointB = matrixB * restraint_member(boss)->pivotB;
	BVector offset = pointB - pointA;
	BVector linearPos = matrixA.inverse().mult3(offset);
	for (int i = 0; i < 3; i++)
	{
		LimitMotor& limit = linearLimits[i];
		limit.limited = false;
		limit.limitError = 0;
		limit.currentValue = linearPos[i];
		if (limit.range.min() > limit.range.max())
			continue;
		BReal pos = linearPos[i];
		if (pos < limit.range.min())
		{
			limit.limited = true;
			limit.limitError = pos - limit.range.min();
			continue;
		}
		if (pos > limit.range.max())
		{
			limit.limited = true;
			limit.limitError = pos - limit.range.max();
			continue;
		}
	}

	calculatedAxis[0] = matrixA.row(0);
	calculatedAxis[1] = matrixA.row(1);
	calculatedAxis[2] = matrixA.row(2);

	calculatedAxis[0].normalize();
	calculatedAxis[1].normalize();
	calculatedAxis[2].normalize();

	BMatrix matrix = matrixB * matrixA.inverse();
	BVector euler = matrix.euler();

	euler.x() = NormalizeAngle(euler.x());
	euler.y() = NormalizeAngle(euler.y());
	euler.z() = NormalizeAngle(euler.z());

	for (int i = 0; i < 3; i++)
	{
		LimitMotor& limit = angularLimits[i];
		limit.limited = false;
		limit.limitError = 0;
		angularLimits[i].currentValue = euler[i];
		if (limit.range.min() > limit.range.max())
			continue;
		BReal angle = euler[i];
		if (angle < limit.range.min())
		{
			limit.limited = true;
			limit.limitError = angle - limit.range.min();
			continue;
		}
		if (angle > limit.range.max())
		{
			limit.limited = true;
			limit.limitError = angle - limit.range.max();
			continue;
		}
	}
}
void member_BSixdofRestraint::applyAngularLimit(BReal stepTime, BSolverItem& solverItem, LimitMotor& limitMotor, const BVector& axis)
{
	bool powered = limitMotor.enabled;
	if (powered || limitMotor.limited)
	{
		solverItem.torqueAxisA = axis;
		solverItem.torqueAxisB = -axis;
		if (limitMotor.limited && (limitMotor.range.min() > limitMotor.range.max()))
			powered = false;
		solverItem.velocity = 0;
		if (powered)
		{
			BReal motorTarget = limitMotor.motorTarget;
			BReal motorVelocity = limitMotor.motorVelocity;
			BReal factor = MotorFactor(limitMotor.currentValue, limitMotor.motorTarget, motorVelocity, stepTime);
			solverItem.velocity -= factor * motorVelocity;
			solverItem.impulseLowerLimit = -limitMotor.motorMaxForce * stepTime;
			solverItem.impulseUpperLimit = limitMotor.motorMaxForce * stepTime;
		}
		if (limitMotor.limited)
		{
			solverItem.velocity += limitMotor.limitError / stepTime;
			solverItem.impulseLowerLimit = -REAL_MAX;
			solverItem.impulseUpperLimit = REAL_MAX;
			if (limitMotor.range.min() < limitMotor.range.max())
			{
				if (limitMotor.limitError < 0)
					solverItem.impulseUpperLimit = 0;
				else
					solverItem.impulseLowerLimit = 0;
			}
		}
	}
}
void member_BSixdofRestraint::applyLinearLimit(BReal stepTime, BSolverItem& solverItem, LimitMotor& limitMotor, const BVector& axis, int rotAllowed)
{
	const BBody* bodyA = restraint_member(boss)->bodyA;
	const BBody* bodyB = restraint_member(boss)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	const BVector& linVelA = body_member(bodyA)->linearVelocity;
	const BVector& linVelB = body_member(bodyB)->linearVelocity;
	const BVector& angVelA = body_member(bodyA)->angularVelocity;
	const BVector& angVelB = body_member(bodyB)->angularVelocity;

	BVector pointA = matrixA * restraint_member(boss)->pivotA;
	BVector pointB = matrixB * restraint_member(boss)->pivotB;

	BReal invMassA = solverItem.invMassA;
	BReal invMassB = solverItem.invMassB;

	BReal invMassSum = invMassA + invMassB;
	BReal factA = BReal(0.5);
	if (invMassSum > BReal(0))
	{
		factA = invMassB / invMassSum;
	}
	BReal factB = 1 - factA;

	bool powered = limitMotor.enabled;
	if (powered || limitMotor.limited)
	{
		solverItem.contactNormalA = axis;
		solverItem.contactNormalB = -axis;

		BVector relB = pointB - matrixB.position();
		BVector projB = axis * relB.dot(axis);
		BVector orthoB = relB - projB;

		BVector relA = pointA - matrixA.position();
		BVector projA = axis * relA.dot(axis);
		BVector orthoA = relA - projA;

		BReal desiredOffs = limitMotor.currentValue - limitMotor.limitError;
		BVector totalDist = projA + axis * desiredOffs - projB;
		relA = orthoA + totalDist * factA;
		relB = orthoB - totalDist * factB;
		BVector tmpA = relA.cross(axis) * factA;
		BVector tmpB = relB.cross(axis) * factB;
		solverItem.torqueAxisA = tmpA;
		solverItem.torqueAxisB = -tmpB;
		solverItem.velocity = 0;
		if (limitMotor.limited && (limitMotor.range.min() > limitMotor.range.max()))
			powered = false;
		if (powered)
		{
			BReal motorTarget = limitMotor.motorTarget;
			BReal motorVelocity = limitMotor.motorVelocity;
			BReal factor = MotorFactor(limitMotor.currentValue, motorTarget, motorVelocity, stepTime);
			solverItem.velocity -= factor * motorVelocity;
			solverItem.impulseLowerLimit = -limitMotor.motorMaxForce * stepTime;
			solverItem.impulseUpperLimit = limitMotor.motorMaxForce * stepTime;
		}
		if (limitMotor.limited)
		{
			solverItem.velocity += limitMotor.limitError / stepTime * 0.2;
			solverItem.impulseLowerLimit = -REAL_MAX;
			solverItem.impulseUpperLimit = REAL_MAX;
			if (limitMotor.range.min() < limitMotor.range.max())
			{
				if (limitMotor.limitError < 0)
					solverItem.impulseUpperLimit = 0;
				else
					solverItem.impulseLowerLimit = 0;
			}
		}
	}
}
